function [ DupsilonBl, DalphaBl, accTriadJfCond, gyroTriadJfCond ] = imuout2in_inc( DupsilonTildeOutSl, DalphaTildeOutSl, IMUPar, Tl, DalphaBl1, innerIterAlgoType, outerIterStepNum, innerIterStepNum ) %#codegen
%IMUOUT2IN_INC 将器件的输出值转换为输入值
%
% Tips
% # 本函数针对增量形式
%
% Input Arguments
% # DupsilonTildeOutSl: 3*n矩阵，各采样周期内加速度计输出脉冲数
% # DalphaTildeOutSl: 3*n矩阵，各采样周期内陀螺输出脉冲数
% # IMUPar: 包含acc域（加速度计三元组）和gyro域（陀螺三元组）的确定性误差参数的结构体，参数详细说明参考accerror_cont及gyroerror_cont函数
% # Tl: 标量，采样周期，单位s
% # DalphaBl1: 3*1向量，DalphaTildeOutSl首周期前一周期的经补偿后的角增量，输入为NaN(3, 1)时则使用首周期值代替首周期前一周期值（用于连续采样）；
%            或3*n矩阵，DalphaTildeOutSl各周期前一周期的经补偿后的角增量（用于不连续采样），单位rad，默认为NaN(3, 1)
% # innerIterAlgoType：2*1向量，int8，加表，陀螺内部迭代使用的算法类型，默认加表为1，陀螺为2
%                innerIterAlgoType = 1，牛顿迭代；
%                innerIterAlgoType = 其它值，线性迭代。
% # outIterStepNum: 标量，外部迭代步数，默认为2
% # innerIterStepNum: 标量，内部迭代步数，仅当使用牛顿迭代时有效，默认为2
%
% Output Arguments
% # DupsilonBl: 3*n矩阵，经补偿后的各采样周期内器件输入比速度增量值，单位m/s
% # DalphaBl: 3*n矩阵，经补偿后的各采样周期内器件输入角增量值，单位rad
% # accTriadJfCond: n*1列向量，加速度计三元组Jf矩阵条件数，无单位
% # gyroTriadJfCond: n*1列向量，陀螺三元组Jf矩阵条件数，无单位
%
% References:
% # 《应用导航算法工程基础》“由输出值计算输入值的算法”

if nargin < 5
    DalphaBl1 = NaN(3, 1);
end
if nargin < 6
    innerIterAlgoType = [int8(1); int8(2)];
end
if nargin < 7
    outerIterStepNum = 2;
end
if nargin < 8
    innerIterStepNum = 2;
end

coder.extrinsic('warning','int2str','num2str');

assert(isequal(size(innerIterAlgoType), size(NaN(2, 1))));
assert(isequal(size(DupsilonTildeOutSl), size(DalphaTildeOutSl)));
assert(isequal(size(IMUPar.acc.KScal0, 2), size(IMUPar.acc.PS0B, 3), size(IMUPar.acc.dfBiasS, 2), size(IMUPar.acc.dKScalP, 3),...
    size(IMUPar.acc.dKScalN, 3), size(IMUPar.acc.dPSS0, 3), size(IMUPar.acc.dfQuantS, 2), size(IMUPar.acc.lB, 3), ...
    size(IMUPar.acc.KAniso, 2), size(IMUPar.acc.CBA, 4)));
assert(isequal(size(IMUPar.gyro.KScal0, 2), size(IMUPar.gyro.PS0B, 3), size(IMUPar.gyro.domegaIBBiasS, 2), size(IMUPar.gyro.dKScalP, 3),...
    size(IMUPar.gyro.dKScalN, 3), size(IMUPar.gyro.dPSS0, 3), size(IMUPar.gyro.domegaIBQuantS, 2), size(IMUPar.gyro.DGSens, 3)));

n = size(DupsilonTildeOutSl, 2);
if n == size(IMUPar.acc.KScal0, 2)
    isIMUParFullSize = true;
else
    isIMUParFullSize = false;
end

DupsilonBl = NaN(size(DupsilonTildeOutSl));
DalphaBl = NaN(size(DalphaTildeOutSl));
if nargout >= 3
    accTriadJfCond = NaN(n, 1);
end
if nargout >= 4
    gyroTriadJfCond = NaN(n, 1);
end
for j=1:n
    %% 计算标度因数误差阵
    if isIMUParFullSize
        idx = j;
    else
        idx = 1;
    end
    accdKScalCoef = scalfacterrsign(IMUPar.acc.dKScalP(:, :, idx), IMUPar.acc.dKScalN(:, :, idx), DupsilonTildeOutSl(:, j));
    gyrodKScalCoef = scalfacterrsign(IMUPar.gyro.dKScalP(:, :, idx), IMUPar.gyro.dKScalN(:, :, idx), DalphaTildeOutSl(:, j));
    
    %% 对输出值按标度因数标称值、零偏进行补偿得到迭代初值
    accPSBT = (IMUPar.acc.PS0B(:, :, idx)*(eye(3)+IMUPar.acc.dPSS0(:, :, idx)))';
    [DupsilonBl0, accTriadJfCond(j, 1)] = sensorout2in_linear(DupsilonTildeOutSl(:, j), 0, zeros(3, 1), accPSBT, ...
        IMUPar.acc.KScal0(:, idx), accdKScalCoef, IMUPar.acc.dfBiasS(:, idx)*Tl, IMUPar.acc.dfQuantS(:, idx)*Tl);
    if accTriadJfCond(j, 1) > 20
        warning(['Jf condition number of acc triad of sample ' int2str(j) ' reaches ' num2str(accTriadJfCond(j, 1))]);
    end
    gyroPSBT = (IMUPar.gyro.PS0B(:, :, idx)*(eye(3)+IMUPar.gyro.dPSS0(:, :, idx)))';
    [DalphaBl0, gyroTriadJfCond(j, 1)] = sensorout2in_linear(DalphaTildeOutSl(:, j), 0, zeros(3, 1), gyroPSBT, ...
        IMUPar.gyro.KScal0(:, idx), gyrodKScalCoef, IMUPar.gyro.domegaIBBiasS(:, idx)*Tl, IMUPar.gyro.domegaIBQuantS(:, idx)*Tl);
    if gyroTriadJfCond(j, 1) > 20
        warning(['Jf condition number of gyro triad of sample ' int2str(j) ' reaches ' num2str(gyroTriadJfCond(j, 1))]);
    end
    
    %% 前一周期角增量
    if size(DalphaBl1, 2) == 1
        if j == 1
            if any(isnan(DalphaBl1))
                DalphaBl1_effective = DalphaBl0;
            else
                DalphaBl1_effective = DalphaBl1;
            end
        else
            DalphaBl1_effective = DalphaBl(:, j-1);
        end
    else % 传入所有周期的前点值，在测试用例中使用
        DalphaBl1_effective = DalphaBl1(:, j);
    end
    
    %% 内外层迭代
    DupsilonBlOuter = DupsilonBl0;
    DalphaBlOuter = DalphaBl0;
    for iOut =1:outerIterStepNum
        %% 计算外层迭代g(v, w, p)
        % 杆臂效应误差-比速度增量形式
        dfSizeSInt = acclevarmerr_inc(accPSBT, IMUPar.acc.lB(:, :, idx), DalphaBlOuter, DalphaBl1_effective, Tl);
        % 不等惯量误差-比速度增量形式
        dfAnisoSInt = accanisoerr_inc(IMUPar.acc.KAniso(:, idx), IMUPar.acc.CBA(:, :, :, idx), DalphaBlOuter, DalphaBl1_effective, Tl);
        % g敏感项误差-角速度形式
        domegaIBGSensSInt = IMUPar.gyro.DGSens(:, :, idx)*DupsilonBlOuter;
        % 计算KScal NOTE: vTildeScalS计算式应与accerror、gyroerror及fgjacobian函数一致
        [~, DupsilonBlKScal] = polyscalfact((accPSBT)*DupsilonBlOuter+IMUPar.acc.dfBiasS(:, idx)*Tl+dfSizeSInt+dfAnisoSInt, ...
            accdKScalCoef, IMUPar.acc.KScal0(:, idx), Tl);
        [~, DalphaBlKScal] = polyscalfact(gyroPSBT*DalphaBlOuter+IMUPar.gyro.domegaIBBiasS(:, idx)*Tl+domegaIBGSensSInt, ...
            gyrodKScalCoef, IMUPar.gyro.KScal0(:, idx), Tl);
        % 计算g(v, w, p)的值
        gDupsilonOuter = DupsilonBlKScal.*(dfSizeSInt+dfAnisoSInt);
        gDalphaOuter = DalphaBlKScal.*domegaIBGSensSInt;
        % 调用加表内层迭代的函数
        switch innerIterAlgoType(1)
            case int8(1)
                DupsilonBlOuter = sensorout2in_newton(DupsilonTildeOutSl(:, j), gDupsilonOuter, DupsilonBlOuter, accPSBT, ...
                    IMUPar.acc.KScal0(:, idx), accdKScalCoef, IMUPar.acc.dfBiasS(:, idx)*Tl, IMUPar.acc.dfQuantS(:, idx)*Tl, dfSizeSInt+dfAnisoSInt, innerIterStepNum, Tl);
            otherwise
                DupsilonBlOuter = sensorout2in_linear(DupsilonTildeOutSl(:, j), gDupsilonOuter, DupsilonBlOuter, accPSBT, ...
                    IMUPar.acc.KScal0(:, idx), accdKScalCoef, IMUPar.acc.dfBiasS(:, idx)*Tl, IMUPar.acc.dfQuantS(:, idx)*Tl, Tl);
        end
        % 调用陀螺内层迭代的函数
        switch innerIterAlgoType(2)
            case int8(1)
                DalphaBlOuter = sensorout2in_newton(DalphaTildeOutSl(:, j), gDalphaOuter, DalphaBlOuter, gyroPSBT, ...
                    IMUPar.gyro.KScal0(:, idx), gyrodKScalCoef, IMUPar.gyro.domegaIBBiasS(:, idx)*Tl, IMUPar.gyro.domegaIBQuantS(:, idx)*Tl, domegaIBGSensSInt, innerIterStepNum, Tl);
            otherwise
                DalphaBlOuter = sensorout2in_linear(DalphaTildeOutSl(:, j), gDalphaOuter, DalphaBlOuter, gyroPSBT, ...
                    IMUPar.gyro.KScal0(:, idx), gyrodKScalCoef, IMUPar.gyro.domegaIBBiasS(:, idx)*Tl, IMUPar.gyro.domegaIBQuantS(:, idx)*Tl, Tl);
        end
    end
    DupsilonBl(:, j) = DupsilonBlOuter;
    DalphaBl(:, j) = DalphaBlOuter;
end
end